/*
 * geometric_control.hpp
 *
 *  Created on: 2021年3月28日
 *      Author: 17900
 */
#if 0

#ifndef MODULES_CONTROL_GEOMETRIC_CONTROL_GEOMETRIC_CONTROL_HPP_
#define MODULES_CONTROL_GEOMETRIC_CONTROL_GEOMETRIC_CONTROL_HPP_

#include "system/system.hpp"
#include "userlib/userlib.hpp"
#include "control/pid/pid.hpp"

#ifdef __cplusplus
extern "C" {
#endif


#ifdef __cplusplus
}
#endif

#ifdef __cplusplus

#include "math.h"
#include <Eigen>

using namespace Eigen;
using namespace std;

class GEOMETRIC_CTRL
{
public:
	GEOMETRIC_CTRL();
	~GEOMETRIC_CTRL();
	void motor_Output();
	void Geometric_ctrl_init();
	void Geometric_ctrl_run();

	void Geometric_ctrl_pos_init();
	void Geometric_ctrl_pos_run();

	void para_updata();

	void flight_mode_update();


private:


	eQUAD_MODE Mode;

	Vector3f position_NED_desired;
	Vector3f position_NED;
	Vector3f error_position_NED;
	Vector3f volcity_NED_desired;
	Vector3f volcity_NED;
	Vector3f error_volcity_NED;
	Vector3f acceleration_NED_desired;

	Vector3f Tc;	//NED坐标系

	float f;
	float gravity;

	Vector3f b3c;
	Vector3f b1d;
	Vector3f b1c;
	Vector3f b2c;


	float SetAng[3];
	float SetAnf_fil[3];
	Quaternion<float> q;

	Matrix<float,3,3> Rd;				//期望姿态
	Matrix<float,3,3> Rd_last;				//期望姿态
	Matrix<float,3,3> R;					//当前姿态
	Vector3f error_R;			//误差姿态
	Vector3f angleRate_desired;	//期望角速度
	Vector3f angleRate_desired_last;	//期望角速度
	Vector3f angleRate;			//当前角速度
	Vector3f error_angleRate;	//误差角速度
	Matrix<float,3,3> angleRate_skewMatrix;								//角速度的反对称矩阵
	Vector3f angleAcceleration_desired;									//角加速度期望值

	Vector3f moments;			//转矩控制器输出
	// 控制器参数
	Matrix<float,3,3> Kx;
	Matrix<float,3,3> Kv;
	Matrix<float,3,3> KR;
	Matrix<float,3,3> Kw;
	//MAV参数：转动惯量
	Matrix<float,3,3> J;
	float mass;
	//桨叶系数
	float Ct;
	float Cm;
	float Ctf;
	float d;
	Vector4f forceAndMoments;
	Matrix<float,4,4> M_f_To_forceAndMoments;
	Vector4f forcesOfMotors;

	float k_force_To_PWM;
	float k_PWM_To_rps;


	float HoldYaw;
	float dt;
	float pos_dt;

	float thr;

	uint32_t startTimer;
	uint32_t stopTimer;
	uint32_t  executionTime_us;

	ahrs_euler_msg ahrsEuler;
	sensor_gyro_msg gyro;
	RC_command_msg rcCommand;
	Motor_PWM_msg motorPWM;
	height_msg height;
	ekf_uwb_pos_vel_msg ekf_uwb_pos_vel;
//	ekf_msg ekf;
	Pid_msg pid_para;

	Inloop_control_msg inloop_control;


	Matrix<float, 8, 1> poly_pos_t;
	Matrix<float, 8, 1> poly_vel_t;
	Matrix<float, 8, 1> poly_acc_t;

	//轨迹信息
	Matrix<float, 3, 8> poly_coef;
	float ts[20];
	float n_1_ts_sum;			//前n-1段轨迹时间之和
	uint8_t n_seg;
	uint8_t nth_seg;			//正在运行第n段轨迹，n从0开始
	uint32_t trajectoryTrackStartTimer_us;
	uint32_t currentTimer_us;
	float currentTime_s;            //当前时间 单位秒 每段轨迹开始跟踪时置零 也就是每段轨迹相对于该段轨迹开始时的时间
	float trajectoryTrackStartToCurrentTime_s;

};


#endif



#endif
#endif /* MODULES_CONTROL_GEOMETRIC_CONTROL_GEOMETRIC_CONTROL_HPP_ */
